function [d, p, t] = point_to_lineSegment(point, line)
AB = line.p2 - line.p1;
AP = point(1:3) - line.p1;
BP = point(1:3) - line.p2;

theta1 = acos(dot(AB, AP) / (norm(AB) * norm(AP)));
theta2 = acos(dot(AB, BP) / (norm(AB) * norm(BP)));

if theta1 > pi/2
    d = norm(AP);
    t = 0;
    p = line.p1;
elseif theta2 < pi/2
    d = norm(BP);
    t = 1;
    p = line.p2;
else
    d = norm(cross(AB, AP)) / norm(AB);
    t = dot(AP, AB) / norm(AB) / norm(AB);
    p = line.p1 + t * (line.p2 - line.p1);
end
